package edu.gatech.cc.liam.geometry.linear;

import java.util.Arrays;

import edu.gatech.cc.liam.core.Globals;
 
public class HyperplaneProjection {

	final private static boolean DEBUG_PRINTS = false;

	private Halfspace theProjectionPlane;
	NPoint scaledNormal;
	double scaledOffset;
	int elimVarPos;
	
	//= new double[theProjectionPlane.length];
	
	// the Projection Plane should be of the form X1*C1 + X2*C2 + ... + XN*CN + CN+1 = 0
	public HyperplaneProjection(Halfspace theProjectionPlane) {
		this(theProjectionPlane, findMaxAbsPos(theProjectionPlane));
	}

	public HyperplaneProjection(Halfspace theProjectionPlane, int varToElim) {
		super();
		assert theProjectionPlane.isDegenerate() == false;
		
		this.theProjectionPlane = theProjectionPlane;
		elimVarPos = varToElim;
		
		scaledNormal = new NPoint(theProjectionPlane.normal);
		double scaleFactor = -1.0 / theProjectionPlane.normal[elimVarPos];
		scaledNormal.scale(scaleFactor);
		scaledOffset = scaleFactor * theProjectionPlane.offset;
		
		if(DEBUG_PRINTS) 
			System.out.println("Eliminating var:" + varToElim);
	}
	
	// finds the variable location with the coefficient of maximum absolute value
	private static int findMaxAbsPos(Halfspace theProjectionPlane) {
		int maxPos = -1;
		double maxVal = -1;
		for(int i=0; i<theProjectionPlane.normal.length; i++) {
			double absVal = Math.abs(theProjectionPlane.normal[i]);
			if(absVal > maxVal) {
				maxVal = absVal;
				maxPos = i;
			}
		}
		return maxPos;
	}
	
	public Halfspace project(Halfspace h) {
		double newOffset = h.offset + (h.normal[elimVarPos] * scaledOffset);
		
		double eliminatedVarValue = h.normal[elimVarPos];	
		double[] newNormal = new double[h.normal.length-1];
		for(int i=0; i<elimVarPos; i++) {
			newNormal[i] = h.normal[i] + (scaledNormal.values[i] * eliminatedVarValue);
		}
		// skip the elimVarPos
		for(int i=elimVarPos+1; i<h.normal.length; i++){
			newNormal[i-1] = h.normal[i] + (scaledNormal.values[i] * eliminatedVarValue);
		}
		return new Halfspace(newNormal, newOffset);
	}
	
	public NPoint project(NPoint p) {
		NPoint rPt;
		assert Math.abs(theProjectionPlane.pointDistanceFromHyperplane(p)) <= Globals.TOLERANCE;
		rPt = p.dropDimension(this.elimVarPos);
		assert rPt.dimensions() == p.dimensions()-1;
		return rPt;
	}
	public PointSet project(PointSet pts) {
		PointSet rP = new PointSet();
		for(NPoint p : pts) {
			rP.add(project(p));
		}
		return rP;
	}
	
//	public NPoint project(NPoint h) {
//		
//		
//		double newOffset = h.offset + (h.normal[elimVarPos] * scaledOffset);
//		
//		double eliminatedVarValue = h.normal[elimVarPos];	
//		double[] newNormal = new double[h.normal.length-1];
//		for(int i=0; i<elimVarPos; i++) {
//			newNormal[i] = h.normal[i] + (scaledNormal.values[i] * eliminatedVarValue);
//		}
//		// skip the elimVarPos
//		for(int i=elimVarPos+1; i<h.normal.length; i++){
//			newNormal[i-1] = h.normal[i] + (scaledNormal.values[i] * eliminatedVarValue);
//		}
//		return new Halfspace(newNormal, newOffset);
//	}
	
	// Each input inequality should be of the form  X1*C1 + X2*C2 + ... + XN*CN + CN+1 <= 0
	// Remove degenerate halves
	public Polytope project(Polytope originalPolytope) {
		Polytope rP = new Polytope();
		for(Halfspace h : originalPolytope){
			Halfspace projectedHalfSpace = project(h);
			if(projectedHalfSpace.isDegenerate() == false) {
				rP.add(projectedHalfSpace);
			} else if (projectedHalfSpace.isInfeasible()) {
				double[] a = null;
				a[0] = 0.0;
//				return new Polytope();
			}
		}
		return rP;
	}
	
	/**
	 * @param p
	 * @return
	 */
	public NPoint unproject(NPoint p) {
		double[] v = new double[p.values.length+1];
		assert(v.length == scaledNormal.values.length);
		
		for(int i=0; i<elimVarPos; i++)
			v[i] = p.values[i];
		// skip the elimVarPos
		for(int i=elimVarPos; i<p.values.length; i++)
			v[i+1] = p.values[i];
		
		NPoint newPoint = new NPoint(v);
		double newVarValue =  (newPoint.dotProduct(scaledNormal) + scaledOffset);
		newPoint.values[elimVarPos] = newVarValue;

		return newPoint;
	}

	public PointSet unproject(PointSet theSet) {
		PointSet newSet = new PointSet();
		for(NPoint aPoint : theSet) {
			newSet.add(unproject(aPoint));
		}
		return newSet;
	}
	
	public static void main(String[] args) {
		double[] n = {1.0, 1.0};
		Halfspace proj = new Halfspace(n, -1.0);
		double[][] hs = { {-2.0, 1.0, 0.0}, {1.0, 0.0, -1.0}, {0.0, -1.0, 0.0} };
		Polytope halfs = new Polytope(hs);
		
		HyperplaneProjection theProj = new HyperplaneProjection(proj,1);
		Polytope resultingHalfs = theProj.project(halfs);
		System.out.println(matrixToString(resultingHalfs.toMatrix()));
		
		double[][] points = { {0.0}, {0.3333333}, {1.0}};
		for(int i=0; i<resultingHalfs.size(); i++) {
			System.out.println(theProj.unproject(new NPoint(points[i])));			
		}
	}

	
	public static String matrixToString (double[][] matrix) {
		String rStr = "{\n";
		for(int i=0; i<matrix.length; i++) {
			rStr += Arrays.toString(matrix[i]) + ", \n";
		}
		return rStr += "}";
	}

	@Override
	public String toString() {
		return "HyperplaneProjection [theProjectionPlane=" + theProjectionPlane
				+ ", scaledNormal=" + scaledNormal + ", scaledOffset="
				+ scaledOffset + ", elimVarPos=" + elimVarPos + "]";
	}

}
